#include "platform.h"
#include "eeprom.h"
#include "board_timer.h"
#include "board_gpio.h"
#include "board_uart.h"
#include "board_config.h"
#include "mpu9250.h"
#include "lis302dl.h"
#include "adxl345.h"
#include "board_dac.h"

extern UART_HandleTypeDef uart1handler;
extern UART_HandleTypeDef uart2handler;
extern SPI_HandleTypeDef spi1handler;

#if 0
MPU9250_HandleTypeDef mpuHandler;
LIS302DL_HandleTypeDef lisHandler;
#endif

ADXL345_HandleTypeDef adxlHandler;

void Error_Handler(void);
void SystemClock_Config(void);

void board_init(void)
{
    /* STM32F4xx HAL library initialization:
       - Configure the Flash prefetch, instruction and Data caches
       - Configure the Systick to generate an interrupt each 1 msec
       - Set NVIC Group Priority to 4
       - Global MSP (MCU Support Package) initialization
       */
    HAL_Init();

    /* -2- Configure the system clock */
    SystemClock_Config();

    my_malloc_init();
    board_led_init();
    eeprom_init();
    board_uart_init(&uart1handler, 115200);
#ifdef BOARD_SBUS_UART
    board_uart_init(&uart2handler, 100000);
#endif

#if BOARD_SPIn > 0
    board_spi_init(&spi1handler);
    adxlHandler.initStructure.Full_Res = ADXL345_FULL_RES;
    adxlHandler.initStructure.Full_Scale = ADXL345_FS_2G;
    adxlHandler.initStructure.Output_Data_Rate = ADXL345_800HZ;
    adxlHandler.initStructure.Power_Mode = ADXL345_PM_NORMAL;
    adxlHandler.initStructure.status = ADXL_UNIDENTIFIED;
    adxlHandler.spi = &spi1handler;
    adxlHandler.init_delay = 10;
    adxlHandler.status = ADXL_INIT;
#endif

#if 0
    mpuHandler.initStructure.clk_sel = MPU9250_CLK_AUTO;
    mpuHandler.initStructure.gyr_fs = MPU9250_GFS_500;
    mpuHandler.initStructure.gyr_lpf_bw = MPU9250_GLPF_BW_250HZ;
    mpuHandler.initStructure.acc_fs = MPU9250_AFS_4G;
    mpuHandler.initStructure.acc_lpf_bw = MPU9250_ALPF_BW_460;
    mpuHandler.spi = &spi1handler;
    mpuHandler.readMagAfterNImu = 100;
    mpuHandler.init_delay = 10;
    mpuHandler.status = INIT;
    mpuHandler.initStructure.status = INIT_RESET;

    lisHandler.initStructure.Power_Mode = LIS302DL_LOWPOWERMODE_ACTIVE;
    lisHandler.initStructure.Output_DataRate = LIS302DL_DATARATE_400;
    lisHandler.initStructure.Axes_Enable = LIS302DL_X_ENABLE |
                                           LIS302DL_Y_ENABLE | LIS302DL_Z_ENABLE;
    lisHandler.initStructure.Full_Scale = LIS302DL_FULLSCALE_2_3;
    lisHandler.initStructure.Self_Test = LIS302DL_SELFTEST_NORMAL;
    lisHandler.initStructure.status = LIS_UNIDENTIFIED;
    lisHandler.spi = &spi1handler;
    lisHandler.init_delay = 10;
    lisHandler.status = LIS_INIT;
#endif

    board_dac_init();
    /* init the timer that triggers sensor reading */
    trigger_timer_init();
}

/**
 * @brief  System Clock Configuration
 *         The system Clock is configured as follow :
 *            System Clock source            = PLL (HSE)
 *            SYSCLK(Hz)                     = 168000000
 *            HCLK(Hz)                       = 168000000
 *            AHB Prescaler                  = 1
 *            APB1 Prescaler                 = 4
 *            APB2 Prescaler                 = 2
 *            HSE Frequency(Hz)              = 8000000
 *            PLL_M                          = 8
 *            PLL_N                          = 336
 *            PLL_P                          = 2
 *            PLL_Q                          = 7
 *            VDD(V)                         = 3.3
 *            Main regulator output voltage  = Scale1 mode
 *            Flash Latency(WS)              = 5
 * @param  None
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_ClkInitTypeDef RCC_ClkInitStruct;
    RCC_OscInitTypeDef RCC_OscInitStruct;

    /* Enable Power Control clock */
    __PWR_CLK_ENABLE();

    /* The voltage scaling allows optimizing the power consumption when the device is
       clocked below the maximum system frequency, to update the voltage scaling value
       regarding system frequency refer to product datasheet.  */
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

    /* Enable HSE Oscillator and activate PLL with HSE as source */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLM = 8;
    RCC_OscInitStruct.PLL.PLLN = 336;
    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
    RCC_OscInitStruct.PLL.PLLQ = 7;
    if(HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2
       clocks dividers */
    RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK
                                   | RCC_CLOCKTYPE_HCLK
                                   | RCC_CLOCKTYPE_PCLK1
                                   | RCC_CLOCKTYPE_PCLK2);
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
    if(HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
    {
        Error_Handler();
    }
}

void Error_Handler(void) {
    while (1) {
    }
}

void board_write_flash(uint16_t addr, uint16_t data)
{
    eeprom_write(addr, data);
}

uint16_t board_read_flash(uint16_t addr)
{
    uint16_t data = 0xffff;
    eeprom_read(addr, &data);
    return data;
}

#ifdef  USE_FULL_ASSERT
/*
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t* file, uint32_t line)
{
    /* User can add his own implementation to report the file name and line number,
     * ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

    /* Infinite loop */
    while (1)
    {
    }
}
#endif
